//
// File: tar_po.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 25-Jun-2024 01:36:24
//

// Include Files
#include "tar_po.h"
#include "cor_cal.h"
#include <cmath>

// Function Definitions
//
// Arguments    : double tX
//                double tY
//                double vL
//                double vR
//                double dt
//                double *dg
//                double *sig
// Return Type  : void
//
void tar_po(double tX, double tY, double vL, double vR, double dt, double *dg,
            double *sig)
{
  double absxk;
  double car_angle;
  double dg_tmp_idx_0;
  double scale;
  double t;
  double tio;
  double y;
  // 定义参数
  scale = 3.3121686421112381E-170;
  absxk = std::abs(tX);
  if (absxk > 3.3121686421112381E-170) {
    y = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    y = t * t;
  }
  absxk = std::abs(tY);
  if (absxk > scale) {
    t = scale / absxk;
    y = y * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    y += t * t;
  }
  y = scale * std::sqrt(y);
  tio = std::acos(tX / y);
  if (tY < 0.0) {
    tio = -tio;
  }
  // 求X, Y, 车角度
  cor_cal((vL + vR) / 2.0, (vR - vL) / 0.196, dt, &y, &scale, &car_angle);
  // 求dg
  dg_tmp_idx_0 = tX - y;
  y = tY - scale;
  scale = 3.3121686421112381E-170;
  // 求sig
  absxk = std::abs(dg_tmp_idx_0);
  if (absxk > 3.3121686421112381E-170) {
    *dg = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    *dg = t * t;
  }
  absxk = std::abs(y);
  if (absxk > scale) {
    t = scale / absxk;
    *dg = *dg * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    *dg += t * t;
  }
  *dg = scale * std::sqrt(*dg);
  *sig = std::acos(
      (std::cos(car_angle) * dg_tmp_idx_0 + std::sin(car_angle) * y) / *dg);
  if (car_angle >= tio) {
    *sig = -*sig;
  }
}

//
// File trailer for tar_po.cpp
//
// [EOF]
//
